## Untitled - By: STM32 - 周四 四月 19 2018
import sensor,image,time,lcd,os
from pyb import UART,Timer,LED

lcd.init()       #Initialize the lcd screen.

# Capture the color thresholds for whatever was in the center of the image.
#r = [(320//2)-(50//2), (240//2)-(50//2), 50, 50] # 50x50 center of QVGA.
r = [[13,21,12,12]
    ,[60,24,12,12]
    ,[102,24,12,12]]

print("Learning thresholds...")
red_threshold   = [36, 94, 49, 113, 20, 63] # Middle L, A, B values.
green_threshold = [40, 97, -114, -52, 3, 113]
yello_threshold = [31, 97, -22, 52, 59, 118]
ALL_threshold   = [50, 50, 0, 0, 0, 0]

redfile   = os.getcwd()+"red_thresholdV1.0.txt"
greenfile = os.getcwd()+"green_thresholdV1.0.txt"
yellofile = os.getcwd()+"yello_thresholdV1.0.txt"

ENABLE = 1
DISABLE = 0

READ_FLAG = 1


#功  能：十进制数据合成
#参  数：gss 按百位 十位 个位存放的list 其中list[0]为百位 list[1]为十位 list[0]为个位
#返回值：合成以后的十进制数据
def gsb_syn(gss, gsb_num):
    if gsb_num == 1 :    #一位数
        return gss[0]
    elif gsb_num == 2 :  #二位数
        return (gss[0]*10 + gss[1])
    elif gsb_num == 3 :  #三位数
        return (gss[0]*100 + gss[1]*10 + gss[2])


#功  能：字符提取
#参  数：stre 字符串
#返回值：int 类型 list
def MyStrExtract(stre):
    tmp_thre = [0, 0, 0, 0, 0, 0]
    tmp_gsb = [0,0,0]
    num = 0
    gsb = 0                 #记录十进制数位数
    plmi = 1                #记录正负号
    for i in stre:
        if "[" in i :
            num = 0
            plmi = 1
        elif "]" in i :
            tmp_thre[num] = gsb_syn(tmp_gsb, gsb)
            if plmi == 1 :
                tmp_thre[num] *= 1
            else :
                tmp_thre[num] *= -1
            num = 0
            gsb = 0
        elif "," in i :
            tmp_thre[num] = gsb_syn(tmp_gsb,gsb)
            if plmi == 1 :
                tmp_thre[num] *= 1
            else :
                tmp_thre[num] *= -1
            num = num + 1
            gsb = 0                     #记录十进制值位数
            plmi = 1
        elif "-" in i :
            plmi= 2                     #表示该元素为负数
        elif " " in i:
            gsb = gsb
        else :
            print(i)
            #tmp_thre[num] += (int(i) << (4*gsb))
            tmp_gsb[gsb] = int(i)
            gsb = gsb + 1
    return tmp_thre


#写文件操作
def mywrite(mpath):
    global red_threshold, green_threshold, yello_threshold
    w_f =  open(mpath,"w")
    if redfile in mpath:
         print("w_red:",str(red_threshold))
         w_f.write(" ")
         w_f.write(str(red_threshold))
    elif greenfile in mpath:
         print("w_green:",str(green_threshold))
         w_f.write(" ")
         w_f.write(str(green_threshold))
    elif yellofile in mpath:
         print("w_yello:",str(yello_threshold))
         w_f.write(" ")
         w_f.write(str(yello_threshold))
    w_f.close()


#读文件操作
def myread(mpath):
    global red_threshold, green_threshold, yello_threshold
    if READ_FLAG == ENABLE:
        r_f = open(mpath,"r")
        if redfile in mpath:
            red_threshold = MyStrExtract(r_f.read())
            print("red:   "+str(red_threshold))
        elif greenfile in mpath:
            green_threshold = MyStrExtract(r_f.read())
            print("green:  "+str(green_threshold))
        elif yellofile in mpath:
            yello_threshold = MyStrExtract(r_f.read())
            print("yello:  "+str(yello_threshold))
        r_f.close()
    else:
        if redfile in mpath:
            red_threshold = my_CDF_train(0)
            display_list("red",red_threshold)
            mywrite(redfile)
        elif greenfile in mpath:
            green_threshold = my_CDF_train(2)
            display_list("green",green_threshold)
            mywrite(greenfile)
        elif yellofile in mpath:
            yello_threshold = my_CDF_train(1)
            display_list("yello",yello_threshold)
            mywrite(yellofile)

def display_list(name, lis_sr):
    for i in lis_sr:
        print(""+name+"  "+str(i))

dist_num = 0
#学习颜色阈值
def my_CDF_train(a):
    global ALL_threshold, dist_num
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QVGA)   # Set frame size to QVGA (320x240)
    sensor.set_auto_whitebal(False)      #关闭白平衡
    sensor.set_auto_gain(False)          #关闭自动增益
    sensor.set_windowing((100,5,128,50)) #x,y,w,h
    #sensor.set_contrast(-3)
    #sensor.set_brightness(-3)
    sensor.set_saturation(3)
    dist_num = dist_num + 1
    for i in range(200):
        img = sensor.snapshot()
        img.draw_rectangle(r[a], color = (255,255,255))
        img.draw_string(0, 0, ""+str(dist_num), color = (255,255,255),scale = 4, mono_space = False)
        lcd.display(img)
    for i in range(60):
        img = sensor.snapshot()#.lens_corr(1.8)
        hist = img.get_histogram(roi=r[a])
        lo = hist.get_percentile(0.01) # Get the CDF of the histogram at the 1% range (ADJUST AS NECESSARY)!
        hi = hist.get_percentile(0.99) # Get the CDF of the histogram at the 99% range (ADJUST AS NECESSARY)!
        # Average in percentile values.
        ALL_threshold[0] = (ALL_threshold[0] + lo.l_value()) // 2
        ALL_threshold[1] = (ALL_threshold[1] + hi.l_value()) // 2
        ALL_threshold[2] = (ALL_threshold[2] + lo.a_value()) // 2
        ALL_threshold[3] = (ALL_threshold[3] + hi.a_value()) // 2
        ALL_threshold[4] = (ALL_threshold[4] + lo.b_value()) // 2
        ALL_threshold[5] = (ALL_threshold[5] + hi.b_value()) // 2
        for blob in img.find_blobs([ALL_threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
            img.draw_rectangle(blob.rect(),color = (255,255,255))
            img.draw_cross(blob.cx(), blob.cy(),color = (255,255,255))
            #img.chrominvar()
            img.draw_rectangle(r[a], color = (255,255,255))
            lcd.display(img)
    return ALL_threshold


myread(redfile)
myread(yellofile)
myread(greenfile)


#自学习颜色阈值
def Auto_Threshold():
    global READ_FLAG,dist_num
    READ_FLAG = 0
    myread(redfile)
    myread(yellofile)
    myread(greenfile)
    READ_FLAG = 1
    dist_num = 0
    myread(redfile)
    myread(yellofile)
    myread(greenfile)


def Color_Check():
    sensor.set_framesize(sensor.QVGA)   # Set frame size to QVGA (320x240)
    sensor.set_windowing((70,0,200,50)) #x,y,w,h
    #sensor.set_contrast(-3)
    #sensor.set_brightness(-3)
    sensor.set_saturation(3)
    img = sensor.snapshot()

    red_num = 0
    yello_num = 0
    green_num = 0
    max_num = 0
    for i in range(1):
        for blob in img.find_blobs([red_threshold],x_stride = 5, y_stride = 5, pixels_threshold=50, area_threshold=10, merge=False, margin=10):
            img.draw_rectangle(blob.rect(),color = (0,0,0))#用矩形标记出目标颜色区域
            img.draw_cross(blob.cx(), blob.cy())
            img.draw_string(blob.cx()-10, blob.cy()+10, "RED", color = (255,0,0),scale = 2, mono_space = False)
            red_num += 1

        for blob in img.find_blobs([yello_threshold],x_stride = 5, y_stride = 5, pixels_threshold=50, area_threshold=10, merge=False, margin=10):
            img.draw_rectangle(blob.rect(),color = (0,0,0))#用矩形标记出目标颜色区域
            img.draw_cross(blob.cx(), blob.cy())
            img.draw_string(blob.cx()-10, blob.cy()+10, "YELLO", color = (255,255,0),scale = 2, mono_space = False)
            yello_num += 1

        for blob in img.find_blobs([green_threshold],x_stride = 10, y_stride = 10, pixels_threshold=100, area_threshold=100, merge=True, margin=10):
            img.draw_rectangle(blob.rect(),color = (0,0,0))#用矩形标记出目标颜色区域
            img.draw_cross(blob.cx(), blob.cy())
            img.draw_string(blob.cx()-10, blob.cy()+10, "GREEN", color = (0,255,0),scale = 2, mono_space = False)
            green_num += 1

    if red_num > yello_num: #排序
        if red_num > green_num:
            max_num = 0x01  #红色
        else:
            max_num = 0x02  #绿色
    elif yello_num > green_num:
        max_num = 0x03      #黄色
    else:
        max_num = 0x02      #绿色
    lcd.display(img)
    return max_num
